#include "carmbox.h"
#include <buola/io/cipendpoint.h>
#include <robot/kuka/ckinematics.h>
#include <buola/tty/output.h>

namespace buola { namespace robot { namespace kuka {

using namespace buola;
    
static int16_t sPort=5001;
static double sIntTime=0.012;
    
CArmBox::CArmBox()
    :   BLoopBox("arm")
    ,   oJoints(this,"joints out",6)
    ,   oPosition(this,"position out")
    ,   oOrientation(this,"orientation out")
    ,   iJoints(this,"joints in",6)
    ,   iPosition(this,"position in")
    ,   iOrientation(this,"orientation in")
    ,   eMode(this,"mode")
    ,   oStatus(this,"status out")
    ,	mSetMaxSpeed(this,"set max speed",std::bind(&CArmBox::SetMaxSpeed,this,_1,_2))
{
    mAcceptor.reset(new io::CSocketAcceptor(io::CIPEndPoint(sPort)));
    mAcceptor->AcceptAsync(std::bind(&CArmBox::OnArmConnected,this,_1));

    InitOutPacket();

    mParams.mAlpha=0.1;
    mParams.mSoftThreshold=0.1;
    mParams.mHardThreshold=0.01;

    mParams.mMaxVelocity[0]=30.0;
    mParams.mMaxVelocity[1]=30.0;
    mParams.mMaxVelocity[2]=30.0;
    mParams.mMaxVelocity[3]=30.0;
    mParams.mMaxVelocity[4]=30.0;
    mParams.mMaxVelocity[5]=30.0;

    mParams.mMaxAcceleration[0]=45.0;
    mParams.mMaxAcceleration[1]=45.0;
    mParams.mMaxAcceleration[2]=45.0;
    mParams.mMaxAcceleration[3]=45.0;
    mParams.mMaxAcceleration[4]=45.0;
    mParams.mMaxAcceleration[5]=45.0;

    mParams.mMaxLinearSpeed=0.02;
    mParams.mMaxRotSpeed=0.2*M_PI/180.0;

    msg_info() << mParams.mMaxLinearSpeed*1000.0/12.0;
    msg_info() << mParams.mMaxRotSpeed*1000.0/12.0;

    mKUKAIPOC=0;
}

void CArmBox::InitOutPacket()
{
    mOutPacket.mDoc.Create(L"Sen");
    
    CXMLNode lRoot=mOutPacket.mDoc.GetRoot();
    
    lRoot.SetAttrib("Type","CoRob");
    lRoot.SetAttrib("xmlns:xsi","http://www.w3.org/2001/XMLSchema-instance");
    lRoot.SetAttrib("xsi:noNamespaceSchemaLocation","ExternalData.xsd");
    
    CXMLNode lDatNode=lRoot.NewChild("Dat");
    
    lDatNode.SetAttrib("TaskType","b");
    
    lDatNode.NewChild("EStr","Info: cajitas server connected!!");

    CXMLNode lRKorr=lDatNode.NewChild("RKorr");
    lRKorr.SetDoubleAttrib("X",0);
    lRKorr.SetDoubleAttrib("Y",0);
    lRKorr.SetDoubleAttrib("Z",0);
    lRKorr.SetDoubleAttrib("A",0);
    lRKorr.SetDoubleAttrib("B",0);
    lRKorr.SetDoubleAttrib("C",0);
    
    mOutPacket.mAKorr=lDatNode.NewChild("AKorr");
    
    CXMLNode lEKorr=lDatNode.NewChild("EKorr");
    lEKorr.SetDoubleAttrib("E1",0);
    lEKorr.SetDoubleAttrib("E2",0);
    lEKorr.SetDoubleAttrib("E3",0);
    lEKorr.SetDoubleAttrib("E4",0);
    lEKorr.SetDoubleAttrib("E5",0);
    lEKorr.SetDoubleAttrib("E6",0);
    
    CXMLNode lTech=lDatNode.NewChild("Tech");
    lTech.SetIntAttrib("x",1);
    lTech.SetDoubleAttrib("p3",0.0);
    lTech.SetDoubleAttrib("p4",0.0);
    lTech.SetDoubleAttrib("p5",0.0);
    lTech.SetDoubleAttrib("p3x1",0.0);
    lTech.SetDoubleAttrib("p4x1",0.0);
    lTech.SetDoubleAttrib("p5x1",0.0);
    lTech.SetDoubleAttrib("p3x2",0.0);
    lTech.SetDoubleAttrib("p4x2",0.0);
    lTech.SetDoubleAttrib("p5x2",0.0);
    lTech.SetDoubleAttrib("p3x3",0.0);
    lTech.SetDoubleAttrib("p4x3",0.0);
    lTech.SetDoubleAttrib("p5x3",0.0);
    
    lDatNode.NewChild("DiO","0");
    mOutPacket.mIPOC=lDatNode.NewChild("IPOC");
}

bool CArmBox::ReadXML()
{
    std::string lReceiveStr;
    char lBuffer[1024];

    do
    {
        ssize_t lLen=mKUKASocket->Receive(lBuffer,1024);
        if(lLen==0)
            return false;
        else if(lLen<0)
            continue;

        lReceiveStr.append(lBuffer,lLen);
    } while(lReceiveStr.find("</Rob>")==std::string::npos);

    CXMLDoc lXML;
    lXML.CollectFrom(lReceiveStr);

    CXMLNode lDataNode=lXML.GetRoot().GetChild("Dat");

    mat::CVecNd<6> lOldJoints=mActualJoints;

    for(CXMLNode lChild=lDataNode.GetFirstChild();lChild;lChild=lChild.GetNext())
    {
        if(lChild.GetName()=="RIst")
        {
            mActualPos.x() = lChild.GetDoubleAttrib("X");
            mActualPos.y() = lChild.GetDoubleAttrib("Y");
            mActualPos.z() = lChild.GetDoubleAttrib("Z");
            mat::C3DRotation lRot;
            lRot.a = lChild.GetDoubleAttrib("A");
            lRot.b = lChild.GetDoubleAttrib("B");
            lRot.c = lChild.GetDoubleAttrib("C");
            mActualRot=mat::CQuat_d(lRot*M_PI/180.0);
        }
        else if(lChild.GetName()=="AIPos")
        {
            mActualJoints[0] = lChild.GetDoubleAttrib("A1");
            mActualJoints[1] = lChild.GetDoubleAttrib("A2");
            mActualJoints[2] = lChild.GetDoubleAttrib("A3");
            mActualJoints[3] = lChild.GetDoubleAttrib("A4");
            mActualJoints[4] = lChild.GetDoubleAttrib("A5");
            mActualJoints[5] = lChild.GetDoubleAttrib("A6");
        }
        else if(lChild.GetName()=="ASPos")
        {
            mGoalJoints[0] = lChild.GetDoubleAttrib("A1");
            mGoalJoints[1] = lChild.GetDoubleAttrib("A2");
            mGoalJoints[2] = lChild.GetDoubleAttrib("A3");
            mGoalJoints[3] = lChild.GetDoubleAttrib("A4");
            mGoalJoints[4] = lChild.GetDoubleAttrib("A5");
            mGoalJoints[5] = lChild.GetDoubleAttrib("A6");
        }
        else if(lChild.GetName()=="IPOC")
        {
            mKUKAIPOC=to_<int>(lChild.GetContent());
        }
    }

    mActualVelocity=mActualJoints-lOldJoints;
    mActualVelocity/=sIntTime;
    mGoalJoints+=mCorrection; //it gets updated with a delay of one frame,
                              //so if we add the previous correction we get the
                              //real value

    mat::CVecNd<6> lJointsRad=mGoalJoints*(M_PI/180.0);
    CArmPose lPose(lJointsRad);
    forward_kinematics(lPose,mGoalPos,mGoalRot);

//    msg_info() << "correction=" << mCorrection << "\n";
//    msg_info() << "velocity=" << mActualVelocity << "\n";
//    msg_info() << "joints=" << mActualJoints << mActualPos << C3DRotation(mActualRot)*180.0/M_PI << "\n";
//    msg_info() << "goal=" << mGoalJoints << mGoalPos << mat::C3DRotation(mGoalRot)*180.0/M_PI << "\n";

    for(int i=0;i<6;i++)
        oJoints.Assign(i,deg2rad(mActualJoints[i]));

    return true;
}

void CArmBox::CalcCorrection()
{
    static int lSinPhase=0;

    mat::CVecNd<6> lOldCorrection=mCorrection;
    mCorrection=mat::CVecNd<6>(nZero);

    if(eMode.Enabled(MODE_CORRECTION))
    {
        msg_info() << "correction mode\n";
        for(int i=0;i<6;i++)
            mCorrection[i]=iJoints.Get(i);
    }
    else if(eMode.Enabled(MODE_CORRECTION_CARTESIAN))
    {
        msg_info() << "cartesian correction mode\n";
        mat::CVec3d lPosDiff=iPosition.Get();
        mat::CQuat_d lRotDiff=iOrientation.Get();

        mat::CVec3d lIncPos=mGoalPos+lPosDiff;
        mat::CQuat_d lIncRot=lRotDiff*mGoalRot;
        
        CArmPose lIncPose;
        inverse_kinematics(lIncPos,lIncRot,CArmPose(mat::CVecNd<6>(mGoalJoints*(M_PI/180.0))),lIncPose);
        
        mat::CVecNd<6> lDiff;
        
        for(int i=0;i<6;i++)
        {
            lDiff[i]=rad2deg(lIncPose[i])-mGoalJoints[i];
        }
        
        msg_info() << lDiff << "\n";

        bool lReachedGoal=true;
        for(int i=0;i<6;i++)
        {
            if(std::abs(lDiff[i])>=mParams.mSoftThreshold)
            {
                lReachedGoal=false;
            }
        }

        mCorrection=lDiff;
        if(!lReachedGoal)
        {
            for(int i=0;i<6;i++)
            {
                if(std::abs(mCorrection[i])>mParams.mMaxVelocity[i]*sIntTime)
                    mCorrection[i]=copysign(mParams.mMaxVelocity[i]*sIntTime,mCorrection[i]);
            }
        }

        oStatus=!lReachedGoal;
    }
    else if(eMode.Enabled(MODE_GOAL))
    {
        msg_info() << "joint goal state\n";
        mat::CVecNd<6> lTarget;
        for(int i=0;i<6;i++)
            lTarget[i]=iJoints.Get(i)*180.0/M_PI;

        msg_info() << lTarget << "\n";

        mat::CVecNd<6> lDiff=lTarget-mGoalJoints; //distance to target

        //calculate the maximum percentage of the current difference that we can move
        double lMinPerc=1.0; 

        for(int i=0;i<6;i++)
        {
            //maximum velocity that allows us to stop
            double lMax=std::min(mParams.mMaxVelocity[i],
                                sqrt(mParams.mMaxAcceleration[i]*std::abs(lDiff[i])/2.0));
            double lStep=sIntTime*lMax;
            lMinPerc=std::min(lMinPerc,lStep/std::abs(lDiff[i]));
        }

        bool lReachedSoft=true;
        bool lReachedHard=true;

        for(int i=0;i<6;i++)
        {
            if(std::abs(lDiff[i])>mParams.mHardThreshold)
                lReachedHard=false;
            if(std::abs(lDiff[i])>mParams.mSoftThreshold)
                lReachedSoft=false;

            mCorrection[i]=lMinPerc*lDiff[i];
            
            //now check that we are not violating the maximum acceleration constraint
            double lA=(mCorrection[i]-lOldCorrection[i])/(sIntTime*sIntTime);
            if(std::abs(lA)>mParams.mMaxAcceleration[i])
            {
                //acceleration is too big
                mCorrection[i]=lOldCorrection[i]+copysign(mParams.mMaxAcceleration[i]*(sIntTime*sIntTime),lA);
            }
        }

        if(lReachedHard)
        {
            mCorrection=nZero;
        }
        else if(lReachedSoft)
        {
            mCorrection=lDiff;
        }

        oStatus=!lReachedHard;
    }
    else if(eMode.Enabled(MODE_GOAL_CARTESIAN))
    {
        msg_info() << "cartesian goal state\n";
        //don't move more than 5% of the total error or there will be
        //stability problems

        mat::CVec3d lTargetPos=iPosition.Get();
        mat::CQuat_d lTargetRot=iOrientation.Get();

        msg_info() << lTargetPos << mat::C3DRotation(lTargetRot)*180.0/M_PI << "\n";

        mat::CVec3d lPosDiff=lTargetPos-mGoalPos;
        double lInvMult=std::max(1.0/0.05,norm(lPosDiff)/mParams.mMaxLinearSpeed);
        lPosDiff/=lInvMult;

        mat::CQuat_d lPath=lTargetRot*conj(mGoalRot);
        msg_info() << lPath;
        double lInvPow=std::max(1.0/0.05,std::abs(arg(lPath))/mParams.mMaxRotSpeed);
        msg_info() << lInvPow;
        lPath=pow(lPath,1.0/lInvPow);
        msg_info() << lPath;
        
        mat::CVec3d lIncPos=mGoalPos+lPosDiff;
        mat::CQuat_d lIncRot=lPath*mGoalRot;

        CArmPose lIncPose;
        inverse_kinematics(lIncPos,lIncRot,CArmPose(mat::CVecNd<6>(mGoalJoints*(M_PI/180.0))),lIncPose);
        
        mat::CVecNd<6> lDiff;
        
        for(int i=0;i<6;i++)
        {
            lDiff[i]=rad2deg(lIncPose[i])-mGoalJoints[i];
        }
        
        bool lReachedGoal=true;
        for(int i=0;i<6;i++)
        {
            if(std::abs(lDiff[i])>=mParams.mSoftThreshold)
            {
                lReachedGoal=false;
            }
        }

        mCorrection=lDiff;
        if(!lReachedGoal)
        {
            for(int i=0;i<6;i++)
            {
                if(std::abs(mCorrection[i])>mParams.mMaxVelocity[i]*sIntTime)
                    mCorrection[i]=copysign(mParams.mMaxVelocity[i]*sIntTime,mCorrection[i]);
            }
        }

        oStatus=!lReachedGoal;
    }
    else if(eMode.Enabled(MODE_SIN_TEST))
    {
        for(int i=0;i<6;i++)
            mCorrection[i]=iJoints.Get(i)*sin(2.0*M_PI/4.0*lSinPhase*sIntTime);

        lSinPhase++;
    }
    else
    {
        lSinPhase=0;
        oStatus=0;
    }

    for(int i=0;i<6;i++)
    {
        if(std::abs(mCorrection[i])>mParams.mMaxVelocity[i]*sIntTime*1.01) //1% tolerance
        {
            msg_warn() << "maximum velocity exceeded for joint " << i << "\n";
            mCorrection[i]=copysign(mParams.mMaxVelocity[i]*sIntTime,mCorrection[i]);
        }
    }
}

bool CArmBox::SendXML()
{
    //it is weird because corrections are already in radians!!
    const double lWeirdFactor=180.0/M_PI;

    mOutPacket.mAKorr.SetDoubleAttrib("A1",mCorrection[0]*lWeirdFactor);
    mOutPacket.mAKorr.SetDoubleAttrib("A2",mCorrection[1]*lWeirdFactor);
    mOutPacket.mAKorr.SetDoubleAttrib("A3",mCorrection[2]*lWeirdFactor);
    mOutPacket.mAKorr.SetDoubleAttrib("A4",mCorrection[3]*lWeirdFactor);
    mOutPacket.mAKorr.SetDoubleAttrib("A5",mCorrection[4]*lWeirdFactor);
    mOutPacket.mAKorr.SetDoubleAttrib("A6",mCorrection[5]*lWeirdFactor);
    
    mOutPacket.mIPOC.SetContent(to_string(mKUKAIPOC));
    
    std::string lString;
    mOutPacket.mDoc.DumpTo(lString);

    mKUKASocket->Send(lString);
    
    return true;
}

void CArmBox::OnLoop()
{
    unique_lock<mutex> lKUKALock(mKUKAMutex);

    while(!mKUKASocket)
    {
        msg_info() << tty::color(tty::COLOR_BLUE,"waiting for arm to connect\n");
        if(mKUKACondVar.wait_for(lKUKALock,chrono::milliseconds(100))==cv_status::timeout)
            return;
    }

    //it would be great if we could sleep around 6 ms for remote realtime control
    //otherwise, we receive the position information of the previous frame
    //but it seem to be impossible to consistently sleep less than 10 ms, which is
    //too much
    //        usleep(6000);
    //how it works now (bad):
    // - the client sends the correction
    // - we receive the correction while waiting at ReadXML (it's where all the
    //     waiting happens, so that's when it happens)
    // - we store the correction to send it the next time we do SendXML
    // - we receive the position, and send it back to the client. It is the
    //     position for the previous frame, but if we waited for the position
    //     after doing the correction, we won't return the position to the client
    //     until just before the having to send the new one, so the client wouldn't
    //     be able to send a correction every frame
    // - It is not something terrible, we only get the position information 12 ms
    //     late in the client, but it should be possible to fix it
    //
    //how it should work:
    // - the client sends the correction
    // - if we receive the correction while waiting at ReadXML, we will send the
    //     position when we receive it, and the client will be able to send a new
    //     correction
    // - we receive the position, send it, and wait 6 ms (half a frame)
    // - we receive the correction while waiting at usleep
    // - we do the correction
    // - 6 ms later we have a new position, send it to the client, and receive the
    //    new correction while waiting at usleep again. everything works.
    //
    //why it doesn't work
    // - because of the many processes running on the machine and the kernel
    //     scheduling scheme, it's impossible to wait for 6 ms. Sometimes it will
    //     work, but other times it will wait 10 or even 15 ms.
    // - maybe it's possible to fix it by changing the priority of the process, but
    //     I don't have enough privileges to do it myself. I'll try to see if it
    //    works, and which is the optimal sleep time.
    // - it should also be possible to change the latency of the kernel, but
    //    maybe it's better to avoid it

    try
    {
        if(mKUKAIPOC)
        {
            CalcCorrection();
            SendXML();
        }

        ReadXML();
    }
    catch(std::exception &pE)
    {
        msg_warn() << "arm stopped... reason: " << pE.what() << "\n";
        mKUKASocket=nullptr;
    }
}

void CArmBox::OnArmConnected(io::PSocket pSocket)
{
    lock_guard<mutex> lLock(mKUKAMutex);
    
    if(mKUKASocket)
    {
        msg_warn() << "arm tried to connect while there is already an arm connected... this is weird, just ignoring\n";
        return;
    }
    
    mKUKASocket.reset(pSocket);
    mKUKACondVar.notify_all();
    mKUKAIPOC=0;
}

void CArmBox::SetMaxSpeed(double pLinear,double pRot)
{
    msg_info() << "new speed " << pLinear << "\n";
    mParams.mMaxLinearSpeed=pLinear/(1000.0/12.0);
    mParams.mMaxRotSpeed=pRot/(1000.0/12.0);
}

/*namespace kuka*/ } /*namespace robot*/ } /*namespace buola*/ }
